#include "IBUS.h"
#include <Arduino.h>

extern uint16_t RCIN[8];
uint16_t s[32];
uint8_t i = 0;

void INIT_IBUS()
{
  Serial.begin(115200);
}

void IBUS_GET()
{
  uint8_t c = Serial.read();
  static uint16_t checksum;
  switch (i)
  {
  case 0:
    if (c == 0x20)
    {
      for (uint8_t j = 0; j < 32; j ++)
      {
        s[j] = 0;
      }
      s[i] = c;
      i ++;
    }
    //Serial.println("0");
    break;

  case 1:
    if (c == 0x40)
    {
      s[i] = c;
      i ++;
    }
    else
    {
      i = 0;
    }
    break;

  default:
    s[i] = c;
    i ++;
    break;

  case 30:
    checksum = 0;
    for (uint8_t j = 0; j < 30; j ++)
    {
      checksum += s[j];
    }
    checksum ^= 0xFFFF;
    if (c == (checksum & 0xFF))
    {
      s[i] = c;
      i ++;
    }
    else
    {
      i = 0;
    }
    break;

  case 31:
    if (c == (checksum >> 8))
    {
      s[i] = c;
      IBUS_CALC();
      i = 0;
    }
    else
    {
      i = 0;
    }
    break;
  }
}

void IBUS_CALC()
{
  for (uint8_t j = 0; j <= 7; j ++)
  {
    RCIN[j] = s[j * 2 + 2] | (s[j * 2 + 3] << 8);
  }
}
